#include "StdAfx.h"
#include "Skeleton.h"

#include "ModelSkeleton.h"

namespace Skeleton {

/*
CPoseData
*/

CPoseData::CPoseData() :
	m_jointCount(0),
	m_pJointsRelative(NULL),
	m_pJointsAbsolute(NULL),
	m_pJointsStatus(NULL),
	m_pMemoryPool(NULL)
{
}

CPoseData::~CPoseData()
{
	AllocationRelease();
}

//

bool CPoseData::AllocateData(uint32 jointCount)
{
	if (m_jointCount == jointCount)
		return true;

	AllocationRelease();

	size_t size = Align(jointCount * sizeof(QuatT), 16) + Align(jointCount * sizeof(QuatT), 16) + Align(jointCount * sizeof(Status4), 16);

	byte* pBase = NULL;
	if (m_pMemoryPool)
	{
		pBase = (byte*)m_pMemoryPool->Allocate(size);
	}
	else
	{
		pBase = (byte*)CryModuleMemalign(size, 16);
	}

	if (!pBase)
	{
		AllocationRelease();
		return false;
	}

	m_pJointsRelative = (QuatT*)SPU_MAIN_PTR(pBase);
	pBase += jointCount * sizeof(QuatT);
	m_pJointsAbsolute = (QuatT*)SPU_MAIN_PTR(pBase);
	pBase += jointCount * sizeof(QuatT);
	m_pJointsStatus = (Status4*)SPU_MAIN_PTR(pBase);

	m_jointCount = jointCount;
	return true;
}

void CPoseData::AllocationRelease()
{
	if (m_pMemoryPool)
	{
		if (m_pJointsRelative)
			m_pMemoryPool->Free(m_pJointsRelative);
	}
	else
	{
		if (m_pJointsRelative)
			CryModuleMemalignFree(m_pJointsRelative);
	}

	m_jointCount = 0;
	m_pJointsRelative = NULL;
	m_pJointsAbsolute = NULL;
	m_pJointsStatus = NULL;
}

uint32 CPoseData::GetAllocationLength() const
{
	return m_jointCount * (sizeof(QuatT) + sizeof(QuatT) + sizeof(Status4));
}

bool CPoseData::Initialize(const CModelSkeleton& skeleton)
{
	MEMSTAT_CONTEXT(EMemStatContextTypes::MSC_Other, 0, "Character Pose Data");

	if (!AllocateData(skeleton.GetJointCount()))
		return false;

	::memcpy(m_pJointsRelative, skeleton.m_poseData.m_pJointsRelative, m_jointCount * sizeof(QuatT));
	::memcpy(m_pJointsAbsolute, skeleton.m_poseData.m_pJointsAbsolute, m_jointCount * sizeof(QuatT));

	return true;
}

bool CPoseData::Initialize(const CPoseData& poseData)
{
	MEMSTAT_CONTEXT(EMemStatContextTypes::MSC_Other, 0, "Character Pose Data Cloned");

	if (!AllocateData(poseData.GetJointCount()))
		return false;

	size_t totalSize = Align(m_jointCount * sizeof(QuatT), 16) + Align(m_jointCount * sizeof(QuatT), 16) + Align(m_jointCount * sizeof(Status4), 16);

	cryMemcpy(m_pJointsRelative, poseData.m_pJointsRelative, totalSize);

	return true;
}

//

void CPoseData::ComputeAbsolutePose(const CModelJoint* pJoints, bool singleRoot)
{
	QuatT* const __restrict pRelativePose = SPU_PTR_SELECT(GetJointsRelative(), gSkeletonRelativePose);
	QuatT* const __restrict pAbsolutePose = SPU_PTR_SELECT(GetJointsAbsolute(), gSkeletonAbsolutePose);

	if (singleRoot)
	{
		CryPrefetch(&pJoints[0].m_idxParent);
		pAbsolutePose[0] = pRelativePose[0];
		for (uint32 i=1; i<m_jointCount; ++i)
		{
#ifndef _DEBUG
			CryPrefetch(&pJoints[i+1].m_idxParent);
			CryPrefetch(&pAbsolutePose[i+4]);
			CryPrefetch(&pRelativePose[i+4]);
#endif
			ANIM_ASSET_ASSERT( pRelativePose[i].q.IsUnit() );
			ANIM_ASSET_ASSERT( pRelativePose[i].IsValid() );
			ANIM_ASSET_ASSERT( pAbsolutePose[pJoints[i].m_idxParent].q.IsUnit() );
			ANIM_ASSET_ASSERT( pAbsolutePose[pJoints[i].m_idxParent].IsValid() );
			pAbsolutePose[i] = pAbsolutePose[pJoints[i].m_idxParent] * pRelativePose[i];
			pAbsolutePose[i].q.NormalizeSafe();
			ANIM_ASSET_ASSERT( pAbsolutePose[i].q.IsUnit()  );
			ANIM_ASSET_ASSERT( pAbsolutePose[i].q.IsValid() );
			ANIM_ASSET_ASSERT( pAbsolutePose[i].t.IsValid() );
		}
	}
	else
	{
		CryPrefetch(&pJoints[0].m_idxParent);
		for (uint32 i=0; i<m_jointCount; ++i)
		{
#ifndef _DEBUG
			CryPrefetch(&pJoints[i+1].m_idxParent);
			CryPrefetch(&pAbsolutePose[i+4]);
			CryPrefetch(&pRelativePose[i+4]);
#endif
			int32 p = pJoints[i].m_idxParent;
			if (p < 0)
				pAbsolutePose[i] = pRelativePose[i];
			else
				pAbsolutePose[i] = pAbsolutePose[p] * pRelativePose[i];
		}
	}
}

SPU_NO_INLINE void CPoseData::ComputeRelativePose(const CModelJoint* pJoints, bool singleRoot)
{
	QuatT* const __restrict pRelativePose = SPU_PTR_SELECT(GetJointsRelative(), gSkeletonRelativePose);
	QuatT* const __restrict pAbsolutePose = SPU_PTR_SELECT(GetJointsAbsolute(), gSkeletonAbsolutePose);

	if (singleRoot)
	{
		pRelativePose[0] = pAbsolutePose[0];
		for (uint32 i=1; i<m_jointCount; ++i)
		{
			int32 p = pJoints[i].m_idxParent;
			pRelativePose[i] = pAbsolutePose[p].GetInverted() * pAbsolutePose[i];
		}
	}
	else
	{
		for (uint32 i=0; i<m_jointCount; ++i)
		{
			int32 p = pJoints[i].m_idxParent;
			if (p < 0)
				pRelativePose[i] = pAbsolutePose[i];
			else
				pRelativePose[i] = pAbsolutePose[p].GetInverted() * pAbsolutePose[i];
		}
	}
}


#ifndef _RELEASE	
uint32 g_result;
bool CPoseData::ValidateRelPose(const CModelJoint* pJoints) const
{
	if (Console::GetInst().ca_Validate)
	{
		QuatT g_pAbsolutePose[256];
		AABB rAABB;
		rAABB.min.Set(+99999.0f,+99999.0f,+99999.0f);
		rAABB.max.Set(-99999.0f,-99999.0f,-99999.0f);
		const QuatT* const __restrict pRelativePose = SPU_PTR_SELECT(GetJointsRelative(), gSkeletonRelativePose);
		for (uint32 i=0; i<m_jointCount; ++i)
		{
			int32 p = pJoints[i].m_idxParent;
			if (p < 0)
				g_pAbsolutePose[i] = pRelativePose[i];
			else
				g_pAbsolutePose[i] = g_pAbsolutePose[p] * pRelativePose[i];
		}

		if (m_jointCount)
		{
			for (uint32 i=1; i<m_jointCount; i++)
				rAABB.Add( g_pAbsolutePose[i].t );
			g_result=uint32(g_pAbsolutePose[m_jointCount-1].q.GetLength()+g_pAbsolutePose[m_jointCount-1].t.GetLength());
			g_result+=uint32(rAABB.min.GetLength());
			g_result+=uint32(rAABB.max.GetLength());
		}
	}
	return 1;
}

bool CPoseData::ValidateAbsPose(const CModelJoint* pJoints) const
{
	if (Console::GetInst().ca_Validate)
	{
		QuatT g_pRelativePose[256];
		AABB rAABB;
		rAABB.min.Set(+99999.0f,+99999.0f,+99999.0f);
		rAABB.max.Set(-99999.0f,-99999.0f,-99999.0f);
		const QuatT* const __restrict pAbsolutePose = SPU_PTR_SELECT(GetJointsAbsolute(), gSkeletonAbsolutePose);
		for (uint32 i=0; i<m_jointCount; ++i)
		{
			int32 p = pJoints[i].m_idxParent;
			if (p < 0)
				g_pRelativePose[i] = pAbsolutePose[i];
			else
				g_pRelativePose[i] = pAbsolutePose[p].GetInverted() * pAbsolutePose[i]*g_pRelativePose[p];
		}

		if (m_jointCount)
		{
			for (uint32 i=1; i<m_jointCount; i++)
				rAABB.Add( pAbsolutePose[i].t );
			g_result=uint32(g_pRelativePose[m_jointCount-1].q.GetLength()+g_pRelativePose[m_jointCount-1].t.GetLength());
			g_result+=uint32(rAABB.min.GetLength());
			g_result+=uint32(rAABB.max.GetLength());
		}
	}
	return 1;
}
#endif
} //namespace Skeleton
